﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Xml.Serialization;
using System.ComponentModel;
using System.IO;
using System.Windows.Forms;
using System.Threading;

namespace BDSim
{
    //, IDynamicUser把这个接口实现去掉了
    public class DynamicUser : UserBaseModel
    {
        public DynamicUser()
        {
            DataProvider.Parent = this;
            DataProvider.DefineCommand();
            Menu.ItemsSource = DataProvider.MenuCommandList;
        }

        #region 字段
        private ETrackGenType _TrackGenType = ETrackGenType.Simulation;
        private Attitude _UserAttitude;
        private List<NodePosVel> _PosVelXYZList = new List<NodePosVel>();
        private List<Attitude> _AttitudeList = new List<Attitude>();

        /// <summary>
        /// 仿真指令的存储路径下的文件名，由右键配置界面设置后给出（针对汽车、飞机、船）

        /// </summary>
        public string SimTrackFileName;

        /// <summary>
        /// 保存的轨迹数据

        /// </summary>
        public List<UserTrajectoryClass> UserTrajectory = new List<UserTrajectoryClass>();

        /// <summary>
        /// 导入的轨迹数据

        /// </summary>
        [XmlIgnore]
        public List<NodePosVel> LoadedTrackData = new List<NodePosVel>();

        /// <summary>
        /// 导入轨迹采样间隔
        /// </summary>
        [XmlIgnore]
        public double LoadedTrackSampleTimespan = 0;

        private string _TrackDataFilePath = "";//轨迹文件使用STK的.e轨迹文件
        /// <summary>
        /// 保存初始工作目录
        /// </summary>
        [XmlIgnore]
        public string startDirectory = Directory.GetCurrentDirectory();

        public double TrackSimStep;//轨迹仿真步长，通过右键弹框赋值

        public double TrackOutStep;//轨迹输出步长，通过右键弹框赋值


        #region 存储数据
        protected string directoryname;//轨迹数据文件夹名称，暂时存储过程数据
        protected string ObsDatadirectoryname;//观测数据文件夹名称

        protected string PVTDatadirectoryname;//定位数据文件夹名称

        protected string TrackDataFilename;//轨迹数据文件的名称，暂时存储过程数据
        protected string[] ObsDataFilename;//接收机观测数据文件的名称，暂时存储过程数据

        protected string[] PVTDataFilename;//接收机定位数据文件的名称，暂时存储过程数据

        #endregion 存储数据


        #endregion 字段



        #region 属性

        [XmlAttribute, DescriptionAttribute("动态数据生成方式，import：外部导入数据；simulation：通过参数配置仿真数据"), CategoryAttribute("基本信息")]
        public ETrackGenType TrackGenType
        {
            get
            {
                return _TrackGenType;
            }
            set
            {
                _TrackGenType = value;
            }
        }

        [Editor(typeof(System.Windows.Forms.Design.FileNameEditor), typeof(System.Drawing.Design.UITypeEditor))]
        [DescriptionAttribute("导入STK生成的.e轨迹文件的名称"), CategoryAttribute("基本信息")]
        public string TrackDataFilePath
        {
            get { return _TrackDataFilePath; }
            set
            {
                if (value.Contains(startDirectory))
                {
                    value = value.Remove(0, startDirectory.Length + 1);
                }
                _TrackDataFilePath = value;
                Directory.SetCurrentDirectory(startDirectory);
            }
        }

        //动态用户当前时刻的姿态信息

        [XmlIgnore, Browsable(false)]
        public Attitude UserAttitude
        {
            get
            {
                return _UserAttitude;
            }
            set
            {
                _UserAttitude = value;
            }
        }

        /// <summary>
        /// 动态用户轨迹数据列表

        /// </summary>
        [Browsable(false)]
        public List<NodePosVel> PosVelXYZList
        {
            get { return _PosVelXYZList; }
            set { _PosVelXYZList = value; }
        }

        /// <summary>
        /// 动态用户姿态列表

        /// </summary>
        [Browsable(false)]
        public List<Attitude> AttitudeList
        {
            get { return _AttitudeList; }
            set { _AttitudeList = value; }
        }

        #endregion 属性


        #region 动态用户数据交互模型

        DynamicUserDataProvider _DataProvider = new DynamicUserDataProvider();

        [XmlIgnore, Browsable(false)]
        public DynamicUserDataProvider DataProvider
        {
            get { return _DataProvider; }
            set { _DataProvider = value; }
        }
        #endregion 动态用户数据交互模型


        #region 导入的轨迹数据插值

        /// <summary>
        /// 用户导入轨迹数据插值,插值阶次为N-1，N为偶数，注：导入的用户轨迹都默认已转为XYZ坐标
        /// </summary>
        /// <param name="sampleTime">采样时间间隔</param>
        /// <returns>用户的按仿真时间间隔插值出来的轨迹</returns>
        public List<UserTrajectoryClass> TrackDataInter(double sampleTime)
        {
            int N = 10;//插值阶次+1
            //计算总的仿真时间
            double TotalTime = 0;
            DateTime startTime = BDSimObject.GlobalStartTime.ToDateTime();
            DateTime stopTime = BDSimObject.GlobalEndTime.ToDateTime();
            TimeSpan diff = stopTime.Subtract(startTime);
            //总的仿真秒数
            TotalTime = diff.TotalSeconds;
            //总的仿真需要的数据个数
            int TotalNumRequiry = (int)Math.Ceiling(TotalTime / sampleTime) + 1;
            //待返回的插值出的用户轨迹变量定义

            List<UserTrajectoryClass> tempUserTrajectoryList = new List<UserTrajectoryClass>();
            //仿真时段内所需的总的数据个数
            int totalDataNum = LoadedTrackData.Count;
            //总的导入的数据的持续时间
            double DataTimeLast = (totalDataNum - 1) * LoadedTrackSampleTimespan;
            //导入的数据按仿真步长最多能给出的数据个数

            int CanGaveDataNum = (int)(DataTimeLast / sampleTime) + 1;

            double[,] TrackData = new double[TotalNumRequiry, 3];
            //默认9阶插值计算

            //if (TrackDataSampleInterval <= sampleTime && totalDataNum >= N)
            if (totalDataNum >= N)
            {
                //仿真采样间隔是原始轨迹数据时间间隔的整数倍时，无需插值，只需将原始轨迹数据按仿真采样间隔进行提取
                if (sampleTime % LoadedTrackSampleTimespan == 0)
                {
                    TrackData = Inter_0(sampleTime, TotalNumRequiry, LoadedTrackData);
                }
                //仿真采样间隔不是原始轨迹数据时间间隔的整数倍时，需要做插值工作

                else
                {
                    TrackData = Inter_1(sampleTime, TotalNumRequiry, LoadedTrackData);
                }

            }
            else if (totalDataNum < N)
            {
                for (int i = 0; i < TotalNumRequiry; i++)
                {
                    TrackData[i, 0] = LoadedTrackData[0].X;
                    TrackData[i, 1] = LoadedTrackData[0].Y;
                    TrackData[i, 2] = LoadedTrackData[0].Z;
                }
            }
            //插值完毕，插值数据暂存在一个数组TrackData中，现取出存进集合中
            UTCTime simTime = new UTCTime();
            simTime = BDSimObject.GlobalStartTime;
            for (int i = 0; i < TotalNumRequiry; i++)
            {
                UserTrajectoryClass tempPosition = new UserTrajectoryClass();
                tempPosition.UserPosXYZ.X = TrackData[i, 0];
                tempPosition.UserPosXYZ.Y = TrackData[i, 1];
                tempPosition.UserPosXYZ.Z = TrackData[i, 2];
                TimeSpaceDll.CAL2UTC(out simTime, 1);
                tempPosition.UserPosXYZ.epoch = simTime;
                TimeSpaceDll.XYZ2BLH(tempPosition.UserPosXYZ, ECFtype.type_CGCS2000, out tempPosition.UserPosBLH);
                tempUserTrajectoryList.Add(tempPosition);
                simTime.AddSecond(BDSimObject.GlobalStepSize);
            }
            return tempUserTrajectoryList;

        }
        /// <summary>
        /// 整数倍时无需插值，直接提取仿真时刻的坐标

        /// </summary>
        /// <param name="sampleTime"></param>
        /// <param name="TotalNumRequiry"></param>
        /// <param name="loadPos"></param>
        /// <returns></returns>
        private double[,] Inter_0(double sampleTime, int TotalNumRequiry, List<NodePosVel> loadPos)
        {
            double[,] TrackData = new double[TotalNumRequiry, 3];
            //仿真时段内所需的总的数据个数
            int totalDataNum = LoadedTrackData.Count;
            //总的导入的数据的持续时间
            double DataTimeLast = totalDataNum * LoadedTrackSampleTimespan;
            //导入的数据按仿真步长最多能给出的数据个数

            int CanGaveDataNum = (int)(DataTimeLast / sampleTime);
            //提取的数据间隔

            int IntN = (int)(sampleTime / LoadedTrackSampleTimespan);
            //如果导入数据足够持续到仿真的结尾时间
            if (CanGaveDataNum >= TotalNumRequiry)
            {
                for (int i = 0; i < TotalNumRequiry; i++)
                {
                    TrackData[i, 0] = LoadedTrackData[i * IntN].X;
                    TrackData[i, 1] = LoadedTrackData[i * IntN].Y;
                    TrackData[i, 2] = LoadedTrackData[i * IntN].Z;
                }
            }
            //如果导入数据不够持续到仿真的结尾时间
            else
            {
                for (int i = 0; i < CanGaveDataNum; i++)
                {
                    TrackData[i, 0] = LoadedTrackData[i * IntN].X;
                    TrackData[i, 1] = LoadedTrackData[i * IntN].Y;
                    TrackData[i, 2] = LoadedTrackData[i * IntN].Z;
                }
                //剩余部分都用最后一个轨迹值代替

                for (int i = CanGaveDataNum; i < TotalNumRequiry; i++)
                {
                    TrackData[i, 0] = LoadedTrackData[totalDataNum - 1].X;
                    TrackData[i, 1] = LoadedTrackData[totalDataNum - 1].Y;
                    TrackData[i, 2] = LoadedTrackData[totalDataNum - 1].Z;
                }

            }

            return TrackData;
        }

        /// <summary>
        /// 非整数倍，需插值计算仿真时刻的坐标
        /// </summary>
        /// <param name="sampleTime"></param>
        /// <param name="TotalNumRequiry"></param>
        /// <param name="loadPos"></param>
        /// <returns></returns>
        private double[,] Inter_1(double sampleTime, int TotalNumRequiry, List<NodePosVel> loadPos)
        {
            int N = 10;
            double[,] TrackData = new double[TotalNumRequiry, 3];
            //仿真时段内所需的总的数据个数
            int totalDataNum = LoadedTrackData.Count;
            //总的导入的数据的持续时间
            double DataTimeLast = (totalDataNum - 1) * LoadedTrackSampleTimespan;
            //导入的数据按仿真步长最多能给出的数据个数

            int CanGaveDataNum = (int)(DataTimeLast / sampleTime) + 1;
            double[] TArray = new double[N];
            double[] FXArray = new double[N];
            double[] FYArray = new double[N];
            double[] FZArray = new double[N];
            //如果导入数据足够持续到仿真的结尾时间
            if (CanGaveDataNum >= TotalNumRequiry)
            {

                for (int i = 0; i < TotalNumRequiry; i++)
                {
                    //要插的点为i*SimStep,要得到要插点左右两边的数据

                    //要插值点所处的位置，也标示前面已经有多少个数据
                    int MiddleN = (int)(i * sampleTime / LoadedTrackSampleTimespan);
                    //如果前面足够5个则可进行向后滑动插值，每次只取N=10个插值节点中第(N/2=)5到(N/2+1=)6个插值节点之间的插值结果，以使结果更精确

                    if (MiddleN >= N / 2 && MiddleN < totalDataNum - N / 2)
                    {
                        int LeftN = MiddleN - (N / 2 - 1);
                        int RightN = MiddleN + N / 2;
                        //赋值TArray
                        for (int j = 0; j < N; j++)
                        {
                            TArray[j] = (LeftN + j) * LoadedTrackSampleTimespan;
                        }
                        //赋值FXArray
                        for (int j = 0; j < N / 2; j++)
                        {
                            FXArray[j] = LoadedTrackData[LeftN + j].X;
                            FXArray[j + N / 2] = LoadedTrackData[MiddleN + j + 1].X;//kj 修改，原代码导致第5、6个值相同// // FXArray[j + N / 2] = Load_PosXYZ_List[MiddleN + j].X;

                            FYArray[j] = LoadedTrackData[LeftN + j].Y;
                            FYArray[j + N / 2] = LoadedTrackData[MiddleN + j + 1].Y;//kj 修改，原代码导致第5、6个值相同//// FYArray[j + N / 2] = Load_PosXYZ_List[MiddleN + j].Y;

                            FZArray[j] = LoadedTrackData[LeftN + j].Z;
                            FZArray[j + N / 2] = LoadedTrackData[MiddleN + j + 1].Z;//kj 修改，原代码导致第5、6个值相同// // FZArray[j + N / 2] = Load_PosXYZ_List[MiddleN + j].Z;
                        }

                        //时间归一化

                        double tmax = TArray[N - 1];
                        double[] t_Poly = new double[N];
                        for (int k = 0; k < N; k++)
                        {
                            t_Poly[k] = 2.0 * (TArray[k] - TArray[0]) / (tmax - TArray[0]) - 1.0;
                        }
                        double x_Poly = 2.0 * (i * sampleTime - TArray[0]) / (tmax - TArray[0]) - 1.0;

                        int PolyOrd = 8;
                        double[] PolyCoe = new double[PolyOrd + 1];

                        CPolyFitDLL.PolyFit(t_Poly, FXArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 0], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FYArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 1], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FZArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 2], PolyOrd);
                    }
                    //开头和结尾无法舞动插值的部分
                    else if (MiddleN < N / 2)
                    {
                        //取最前面的所有的数据再加上后面的数据凑出N个进行插值 
                        for (int j = 0; j < N; j++)
                        {
                            TArray[j] = j * LoadedTrackSampleTimespan;
                            FXArray[j] = LoadedTrackData[j].X;
                            FYArray[j] = LoadedTrackData[j].Y;
                            FZArray[j] = LoadedTrackData[j].Z;
                        }
                        //时间归一化

                        double tmax = TArray[N - 1];
                        double[] t_Poly = new double[N];
                        for (int k = 0; k < N; k++)
                        {
                            t_Poly[k] = 2.0 * (TArray[k] - TArray[0]) / (tmax - TArray[0]) - 1.0;
                        }
                        double x_Poly = 2.0 * (i * sampleTime - TArray[0]) / (tmax - TArray[0]) - 1.0;

                        int PolyOrd = 8;
                        double[] PolyCoe = new double[PolyOrd + 1];

                        CPolyFitDLL.PolyFit(t_Poly, FXArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 0], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FYArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 1], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FZArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 2], PolyOrd);

                        //CInterpolateDLL.Lagrange(i * sampleTime, N, TArray, FXArray, ref TrackData[i, 0]);
                        //CInterpolateDLL.Lagrange(i * sampleTime, N, TArray, FYArray, ref TrackData[i, 1]);
                        //CInterpolateDLL.Lagrange(i * sampleTime, N, TArray, FZArray, ref TrackData[i, 2]);
                    }
                    else if (MiddleN >= totalDataNum - N / 2)
                    {
                        //取最后面的所有的数据再加上后面的数据凑出N个进行插值 
                        int left = LoadedTrackData.Count - N;
                        for (int j = 0; j < N; j++)
                        {
                            TArray[j] = (left + j) * LoadedTrackSampleTimespan;
                            FXArray[j] = LoadedTrackData[left + j].X;
                            FYArray[j] = LoadedTrackData[left + j].Y;
                            FZArray[j] = LoadedTrackData[left + j].Z;
                        }
                        //时间归一化

                        double tmax = TArray[N - 1];
                        double[] t_Poly = new double[N];
                        for (int k = 0; k < N; k++)
                        {
                            t_Poly[k] = 2.0 * (TArray[k] - TArray[0]) / (tmax - TArray[0]) - 1.0;
                        }
                        double x_Poly = 2.0 * (i * sampleTime - TArray[0]) / (tmax - TArray[0]) - 1.0;

                        int PolyOrd = 8;
                        double[] PolyCoe = new double[PolyOrd + 1];

                        CPolyFitDLL.PolyFit(t_Poly, FXArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 0], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FYArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 1], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FZArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 2], PolyOrd);
                    }
                }
            }
            //导入数据不够
            else
            {

                for (int i = 0; i < CanGaveDataNum; i++)
                {
                    //要插的点为i*SimStep,要得到要插点左右两边的数据

                    //要插值点所处的位置，也标示前面已经有多少个数据
                    int MiddleN = (int)(i * sampleTime / LoadedTrackSampleTimespan);
                    //如果前面足够5个则可进行向后滑动插值，每次只取N=10个插值节点中第(N/2=)5到(N/2+1=)6个插值节点之间的插值结果，以使结果更精确

                    if (MiddleN >= N / 2 && MiddleN < totalDataNum - N / 2)
                    {
                        int LeftN = MiddleN - (N / 2 - 1);
                        //  int LeftN = MiddleN - (N / 2 );
                        int RightN = MiddleN + N / 2;
                        //赋值TArray
                        for (int j = 0; j < N; j++)
                        {
                            TArray[j] = (LeftN + j) * LoadedTrackSampleTimespan;
                        }
                        for (int j = 0; j < N; j++)
                        {
                            FXArray[j] = LoadedTrackData[LeftN + j].X;
                            FYArray[j] = LoadedTrackData[LeftN + j].Y;
                            FZArray[j] = LoadedTrackData[LeftN + j].Z;
                        }
                        //时间归一化

                        double tmax = TArray[N - 1];
                        double[] t_Poly = new double[N];
                        for (int k = 0; k < N; k++)
                        {
                            t_Poly[k] = 2.0 * (TArray[k] - TArray[0]) / (tmax - TArray[0]) - 1.0;
                        }
                        double x_Poly = 2.0 * (i * sampleTime - TArray[0]) / (tmax - TArray[0]) - 1.0;

                        int PolyOrd = 8;
                        double[] PolyCoe = new double[PolyOrd + 1];

                        CPolyFitDLL.PolyFit(t_Poly, FXArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 0], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FYArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 1], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FZArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 2], PolyOrd);


                    }
                    //开头和结尾无法舞动插值的部分
                    else if (MiddleN < N / 2)
                    {
                        //取最前面的所有的数据再加上后面的数据凑出N个进行插值 
                        for (int j = 0; j < N; j++)
                        {
                            TArray[j] = j * LoadedTrackSampleTimespan;
                            FXArray[j] = LoadedTrackData[j].X;
                            FYArray[j] = LoadedTrackData[j].Y;
                            FZArray[j] = LoadedTrackData[j].Z;
                        }
                        //时间归一化

                        double tmax = TArray[N - 1];
                        double[] t_Poly = new double[N];
                        for (int k = 0; k < N; k++)
                        {
                            t_Poly[k] = 2.0 * (TArray[k] - TArray[0]) / (tmax - TArray[0]) - 1.0;
                        }
                        double x_Poly = 2.0 * (i * sampleTime - TArray[0]) / (tmax - TArray[0]) - 1.0;

                        int PolyOrd = 8;
                        double[] PolyCoe = new double[PolyOrd + 1];

                        CPolyFitDLL.PolyFit(t_Poly, FXArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 0], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FYArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 1], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FZArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 2], PolyOrd);
                        //CInterpolateDLL.Lagrange(i * sampleTime, N, TArray, FXArray, ref TrackData[i, 0]);
                        //CInterpolateDLL.Lagrange(i * sampleTime, N, TArray, FYArray, ref TrackData[i, 1]);
                        //CInterpolateDLL.Lagrange(i * sampleTime, N, TArray, FZArray, ref TrackData[i, 2]);
                    }
                    else if (MiddleN >= totalDataNum - N / 2)
                    {
                        //取最后面的所有的数据再加上后面的数据凑出N个进行插值 
                        int left = LoadedTrackData.Count - N;
                        for (int j = 0; j < N; j++)
                        {
                            TArray[j] = (left + j) * LoadedTrackSampleTimespan;
                            FXArray[j] = LoadedTrackData[left + j].X;
                            FYArray[j] = LoadedTrackData[left + j].Y;
                            FZArray[j] = LoadedTrackData[left + j].Z;
                        }
                        //时间归一化

                        double tmax = TArray[N - 1];
                        double[] t_Poly = new double[N];
                        for (int k = 0; k < N; k++)
                        {
                            t_Poly[k] = 2.0 * (TArray[k] - TArray[0]) / (tmax - TArray[0]) - 1.0;
                        }
                        double x_Poly = 2.0 * (i * sampleTime - TArray[0]) / (tmax - TArray[0]) - 1.0;

                        int PolyOrd = 8;
                        double[] PolyCoe = new double[PolyOrd + 1];

                        CPolyFitDLL.PolyFit(t_Poly, FXArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 0], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FYArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 1], PolyOrd);

                        CPolyFitDLL.PolyFit(t_Poly, FZArray, N, PolyOrd, PolyCoe);
                        CPolyFitDLL.PolyVal2(PolyCoe, x_Poly, ref TrackData[i, 2], PolyOrd);

                    }

                }
                //由于导入的用户轨迹的持续时间不足以产生仿真时间段的所有数据，
                //不足的部分都取用户导入轨迹的最后一个时刻的轨迹
                for (int i = CanGaveDataNum; i < TotalNumRequiry; i++)
                {
                    TrackData[i, 0] = LoadedTrackData[totalDataNum - 1].X;
                    TrackData[i, 1] = LoadedTrackData[totalDataNum - 1].Y;
                    TrackData[i, 2] = LoadedTrackData[totalDataNum - 1].Z;
                }

            }

            return TrackData;
        }

        #endregion 导入的轨迹数据插值




        public override void Init()
        {
            base.Init();

            #region 参数初始化

            UserTrajectory.Clear();

            #region 存储数据
            string defaultpath = Directory.GetCurrentDirectory() + "\\DataRecords\\用户段\\动态用户\\";//zly2022
            DateTime NowTime = DateTime.Now;
            string NowTimeString = NowTime.Year.ToString() + string.Format("{0:D2}", NowTime.Month) + string.Format("{0:D2}", NowTime.Day) +
                "_" + string.Format("{0:D2}", NowTime.Hour) + "_" + string.Format("{0:D2}", NowTime.Minute) + "_" + string.Format("{0:D2}", NowTime.Second);

            directoryname = defaultpath + "自定义数据_" + NowTimeString + "\\";
            if (!Directory.Exists(directoryname))
            {
                Directory.CreateDirectory(directoryname);
            }

            //观测数据、定位数据文件名赋值

            ObsDataFilename = new string[RecCollection.Count()];
            PVTDataFilename = new string[RecCollection.Count()];

            ObsDatadirectoryname = directoryname + "观测数据\\";
            PVTDatadirectoryname = directoryname + "定位数据\\";
            if (!Directory.Exists(ObsDatadirectoryname))
            {
                Directory.CreateDirectory(ObsDatadirectoryname);
            }
            if (!Directory.Exists(PVTDatadirectoryname))
            {
                Directory.CreateDirectory(PVTDatadirectoryname);
            }
            for (int i = 0; i < RecCollection.Count(); i++)
            {
                ObsDataFilename[i] = ObsDatadirectoryname + string.Format("{0}号接收机观测数据.txt", RecCollection[i].Id);
                PVTDataFilename[i] = PVTDatadirectoryname + string.Format("{0}号接收机定位数据.txt", RecCollection[i].Id);
            }
            #endregion 存储数据

            #endregion 参数初始化


            #region 轨迹姿态数据仿真或导入
            if (ETrackGenType.Simulation == TrackGenType)
            {
                DynamicSim();//轨迹数据仿真
            }

            if (ETrackGenType.Import == TrackGenType)//根据外部导入的轨迹数据插值
            {
                Attitude userattitude = new Attitude();

                if (!File.Exists(TrackDataFilePath))
                {
                    if (0 == LoadedTrackData.Count())
                    {
                        MessageBox.Show("请导入轨迹数据！");
                        Thread thd = Thread.CurrentThread;
                        BDSimObject.GlobalStatus = ESimStatus.UnInited;
                        thd.Abort();
                    }

                    else
                    {                     
                        UserTrajectory = TrackDataInter(BDSimObject.GlobalStepSize);
                        for (int i = 0; i < UserTrajectory.Count(); i++)
                        {
                            PosVelXYZList.Add(UserTrajectory[i].UserPosXYZ);
                        }

                        if (AttitudeList.Count == 0)//暂时不考虑外部导入的姿态，姿态参数赋值为0，以后有外部导入姿态时再进行改正
                        {
                            for (int i = 0; i < PosVelXYZList.Count(); i++)
                            {
                                userattitude.Yaw = 0.0;//姿态数据赋值
                                userattitude.Pitch = 0;
                                userattitude.Roll = 0;
                                AttitudeList.Add(userattitude);
                            }

                        }
                    }
                }

                else
                {
                    Read_e_File(TrackDataFilePath);
                    if (LoadedTrackData.Count <= 0)
                    {
                        MessageBox.Show(".e格式的轨迹数据可用个数为0！");
                        Thread thd = Thread.CurrentThread;
                        BDSimObject.GlobalStatus = ESimStatus.UnInited;
                        thd.Abort();
                    }
                    else
                    {
                        UserTrajectory = TrackDataInter(BDSimObject.GlobalStepSize);
                        for (int i = 0; i < UserTrajectory.Count(); i++)
                        {
                            PosVelXYZList.Add(UserTrajectory[i].UserPosXYZ);
                        }
                        if (AttitudeList.Count == 0)//暂时不考虑外部导入的姿态，姿态参数赋值为0，以后有外部导入姿态时再进行改正
                        {
                            for (int i = 0; i < PosVelXYZList.Count(); i++)
                            {
                                userattitude.Yaw = 0.0;//姿态数据赋值

                                userattitude.Pitch = 0;
                                userattitude.Roll = 0;

                                AttitudeList.Add(userattitude);
                            }

                        }
                    }
                }
            }
            #endregion 轨迹姿态数据仿真或导入

            PosVelXYZ = PosVelXYZList[0];
            UserAttitude = AttitudeList[0];

            foreach (var item in RecCollection)
            {
                item.Parent = this;
                item.Init();
            }

            //初始化完成反馈任务执行量
            DlgAddCompleteMission();

        }

        public void Read_e_File(string filePath)
        {
            if (File.Exists(filePath))
            {
                double x, y, z;
                double time; //e文件数据时刻点

                string[] AllDatas = File.ReadAllLines(filePath);
                for (int i = 0; i < AllDatas.Count(); i++)
                {
                    try
                    {
                        string[] strList = AllDatas[i].Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
                        if (strList.Count() >= 4 && double.TryParse(strList[0], out time))
                        {
                            //
                            if (LoadedTrackSampleTimespan == 0)
                            {
                                string[] strList_new = AllDatas[i + 1].Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
                                LoadedTrackSampleTimespan = Convert.ToDouble(strList_new[0]) - Convert.ToDouble(strList[0]);
                            }
                            double.TryParse(strList[1], out x);
                            double.TryParse(strList[2], out y);
                            double.TryParse(strList[3], out z);
                            LoadedTrackData.Add(new NodePosVel() { X = x, Y = y, Z = z });
                        }
                    }
                    catch (Exception)
                    {
                        //出现错误继续
                    }
                }
            }
        }


        public virtual void DynamicSim()
        {

        }
    }
}
